use crate::hardware::{
    Track, Interposer, COBConnector
};

use std::collections::{
    VecDeque, HashMap, HashSet
};

pub fn route_track_to_track(interposer: &Interposer, start: Track, end: Track) -> Option<Vec<Track>> {
    let mut queue = VecDeque::<Track>::new();
    let mut used = HashSet::new();
    let mut prev_track: HashMap<Track, (Track, _)> = HashMap::<Track, (Track, COBConnector)>::new();

    queue.push_back(start);
    used.insert(start);

    while let Some(track) = queue.pop_front() {
        if track == end {
            let mut path = Vec::new();
            let mut cur_track = end;
            while let Some((prev_track, connector)) = prev_track.get(&cur_track) {
                path.push(cur_track);
                connector.connect();
                cur_track = *prev_track;
            }
            path.push(cur_track);
            assert_eq!(start, cur_track);

            return Some(path.into_iter().rev().collect());
        }

        // Expand
        for (next_track, connector) in interposer.adjacent_idle_tracks(&track) {
            if used.contains(&next_track) {
                continue;
            } else {
                queue.push_back(next_track);
                used.insert(next_track);
                prev_track.insert(next_track, (track, connector));
            }
        }
    }

    None
}

pub fn route_track_path_to_track(interposer: &Interposer, start: Track, end: Track) -> Option<Vec<Track>> {
    let tracks = interposer.connected_tracks(&start);
    println!("Find full track path:");
    for t in &tracks {
        println!("{:?}", t);
    }

    let mut queue = VecDeque::<Track>::new();
    let mut used = HashSet::new();
    let mut prev_track: HashMap<Track, (Track, _)> = HashMap::<Track, (Track, COBConnector)>::new();

    for t in tracks {
        queue.push_back(t);
        used.insert(t);
    }

    while let Some(track) = queue.pop_front() {
        if track == end {
            let mut path = Vec::new();
            let mut cur_track = end;
            while let Some((prev_track, connector)) = prev_track.get(&cur_track) {
                path.push(cur_track);
                connector.connect();
                cur_track = *prev_track;
            }
            path.push(cur_track);
            // assert_eq!(start, cur_track);

            return Some(path.into_iter().rev().collect());
        }

        // Expand
        for (next_track, connector) in interposer.adjacent_idle_tracks(&track) {
            if used.contains(&next_track) {
                continue;
            } else {
                queue.push_back(next_track);
                used.insert(next_track);
                prev_track.insert(next_track, (track, connector));
            }
        }
    }

    None
}